#include <iostream>
#include <iomanip>
#include <memory>
#include <unistd.h>  // usleep
#include "kinco.hpp"
#include "zeroerr.hpp"
#include "dispatch.h"

using namespace CANalyst2;

static std::shared_ptr<CANDispatch> p_dispatch_ch0 = nullptr;
static std::shared_ptr<CANDispatch> p_dispatch_ch1 = nullptr;
static std::shared_ptr<Kinco> p_kinco_driver = nullptr;
static std::shared_ptr<ZeroErr> p_zeroerr_driver = nullptr;
static float curr_pos = 0.0, current_ap = 0.0, current_rms = 0.0;

static void print_servo_status(void);
static void kinco_servo_test(void);
static void zeroerr_servo_test(void);

static void kinco_servo_test(void)
{
    std::cout << "Set pos mode" << std::endl;
    p_kinco_driver->SetMode(true);

    std::cout << "Servo motor setup" << std::endl;
    p_kinco_driver->Setup();

    std::cout << "Enable Servo motor" << std::endl;
    if(p_kinco_driver->SendEnableMotorPDO() == 0) {
        std::cout << "Enable Servo motor successfully." << std::endl;
    }

    std::cout << "PDO move target abs pos: 0" << std::endl;
    p_kinco_driver->SendMoveTargetPosPDO(0);
    while(1)
    {
        print_servo_status();
        if(abs(curr_pos) < 5)
        {
            break;
        }
        sleep(1);
    }

    std::cout << "PDO move target pos: 65536*500" << std::endl;
    p_kinco_driver->SendMoveTargetPosPDO(65536*500);

    sleep(1);
    print_servo_status();

    p_kinco_driver->SendMoveTargetPosPDO(65536*500, 2000);
    while(1)
    {
        print_servo_status();
        if(curr_pos >= 65536 * 5)
        {
            break;
        }
        sleep(1);
    }

    sleep(1);
    print_servo_status();

    std::cout << "PDO move target pos: -65536*500" << std::endl;
    p_kinco_driver->SendMoveTargetPosPDO(-65536*500);
    while(1)
    {
        print_servo_status();
        if(curr_pos <= -65536 * 500)
        {
            break;
        }
        sleep(1);
    }

    std::cout << "PDO move target abs pos: 0" << std::endl;
    p_kinco_driver->SendMoveTargetPosPDO(0, 120);
    // while(1)
    // {
    //     print_servo_status();
    //     if(abs(curr_pos) < 5)
    //     {
    //         break;
    //     }
    //     sleep(1);
    // }

    // if(p_kinco_driver->SendDisableMotorPDO() == 0) {
    //     std::cout << "Disable servo motor successfully." << std::endl;
    // }
}

static void print_servo_status(void)
{    
    curr_pos = 0.0;
    current_ap = 0.0;
    current_rms = 0.0;
    std::cout << "Print current servo status" << std::endl;
    p_kinco_driver->GetCurFBKPDO(curr_pos, current_ap, current_rms);
    std::cout << std::fixed << std::setprecision(3)
              << "Servo pos:" << curr_pos / 65536
              << " rnds, current_ap:" << current_ap
              << " Ap, current_rms:" << current_rms 
              << " Ap"<< std::endl;         
}

static void zeroerr_servo_test(void)
{
    p_zeroerr_driver->Setup();

    p_zeroerr_driver->SendEnableMotorPDO();

    p_zeroerr_driver->SendWriteQuickStopOptionCode(2);

    int32_t pos = 0;
    bool result = false;
    p_zeroerr_driver->SendAcquireActualPosSDO(pos, result);
    if(result)
    {
        std::cout << std::dec << "Actual pos is:" << pos << ", degree is:" <<  std::fixed << std::setprecision(2) <<p_zeroerr_driver->PosToDegree(pos) << std::endl;
    }
    else
    {
        std::cout << "Acquire actual pos failed." << std::endl;
    }

    std::cout << "Move to 120 degree" << std::endl;
    p_zeroerr_driver->SendMoveTargetPosPDO(120);

    // uint16_t stop_option = 0;
    // result = false;
    // p_zeroerr_driver->SendReadQuickStopOptionSDO(stop_option, result);
    // if(result)
    // {
    //     std::cout << std::dec << "Stop option is:" << stop_option << std::endl;
    // }
    // else
    // {
    //     std::cout << "Acquire stop option failed." << std::endl;
    // }
    for(int i = 0; i < 10 ; i++)
    {
        pos = 0;
        pos = p_zeroerr_driver->GetCurPosPDO();
        std::cout << std::dec << "Actual pos is:" << pos << ", degree is:" <<  std::fixed << std::setprecision(2) << p_zeroerr_driver->PosToDegree(pos) << std::endl;
        sleep(1);
    }

    std::cout << "Send Quick Stop." << std::endl;
    p_zeroerr_driver->SendQuickStopSDO();
    pos = 0;
    result = false;
    p_zeroerr_driver->SendAcquireActualPosSDO(pos, result);
    if(result)
    {
        std::cout << std::dec << "Actual pos is:" << pos << ", degree is:" <<  std::fixed << std::setprecision(2) << p_zeroerr_driver->PosToDegree(pos) << std::endl;
    }
    else
    {
        std::cout << "Acquire actual pos failed." << std::endl;
    }

    sleep(5);

    std::cout << "Send Resume From Quick Stop." << std::endl;
    p_zeroerr_driver->SendResumeFromQuickStopSDO();
    std::cout << "Move to 240 degree" << std::endl;
    p_zeroerr_driver->SendSetProfiledVelocitySDO(18);
    p_zeroerr_driver->SendMoveTargetPosPDO(240);

    for(int i = 0; i < 10 ; i++)
    {
        pos = 0;
        pos = p_zeroerr_driver->GetCurPosPDO();
        std::cout << std::dec << "Actual pos is:" << pos << ", degree is:" <<  std::fixed << std::setprecision(2) << p_zeroerr_driver->PosToDegree(pos) << std::endl;
        sleep(1);
    }

    p_zeroerr_driver->SendSetProfiledVelocitySDO(30);
    std::cout << "Move to 360 degree" << std::endl;
    p_zeroerr_driver->SendMoveTargetPosPDO(360);

    for(int i = 0; i < 10 ; i++)
    {
        pos = 0;
        pos = p_zeroerr_driver->GetCurPosPDO();
        std::cout << std::dec << "Actual pos is:" << pos << ", degree is:" <<  std::fixed << std::setprecision(2) << p_zeroerr_driver->PosToDegree(pos) << std::endl;
        sleep(1);
    }

    std::cout << "Move to 0 degree" << std::endl;
    p_zeroerr_driver->SendMoveTargetPosPDO(0);

    while(pos > 10)
    {
        pos = 0;
        pos = p_zeroerr_driver->GetCurPosPDO();
        std::cout << std::dec << "Actual pos is:" << pos << ", degree is:" <<  std::fixed << std::setprecision(2) << p_zeroerr_driver->PosToDegree(pos) << std::endl;
    }
}

int main()
{
    p_dispatch_ch0 = std::make_shared<CANDispatch>("usbcan", CanBaud::BAUD_500K, 0);
    if (p_dispatch_ch0->valid() != true) {
        std::cerr << "CAN ch0 not initialized" << std::endl;
        return -1;
    }

    p_dispatch_ch1 = std::make_shared<CANDispatch>("usbcan", CanBaud::BAUD_1M, 1);
    if (!p_dispatch_ch1->valid()) {
        std::cerr << "CAN ch1 not initialized" << std::endl;
        return -1;
    }

    p_kinco_driver = std::make_shared<Kinco>(p_dispatch_ch0, 0, 1, true, 65536, 200, 100, 100);
    p_zeroerr_driver = std::make_shared<ZeroErr>(p_dispatch_ch1, 1, 1, true, 524288, 5, 10, 10);

    kinco_servo_test();
    zeroerr_servo_test();

    return 0;
}
